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ABSTRACT 


A  review  of  some  estimation  basics  is  followed  by  illustrative  application^  of  Kalmar, 
fillers  for  stationary  and  maneuvering  targets.  The  variable  dimension  Kalman  filter  is 
used  for  the  maneuvering  target.  The  performance  of  the  nearest  neighbor  standard  filter 
is  compared  to  that  of  the  probabilistic  data  association  filter  for  tracking  a  target  in 
clutter.  Multi-target  tracking,  using  sonar  sensors  to  estimate  an  autonomous  robot's 
distance  from  walls,  is  applied  to  the  navigation  problem.  The  Kalman  filter  equation-: 
can  be  completely  decentralized  and  distributed  among  the  nodes  of  a  multi-sensor  system. 
Each  sending  node  implements  its  own  local  Kalman  filter,  arrives  at  a  partial  decision,  and 
broadcasts  it  to  every  other  node.  Each  nod'-  then  assimilates  this  received  information 
to  arrive  at  its  own  local  but  optimal  estimate  of  the  system  state.  An  appendix  contains 
brief  impl^mentationa!  notes. 


1  INTRODUCTION  AND  BACKGROUND 

This  report  includes  work  that  sprang  directly  out  of  a  reading  group  organized  by  Hugh 
Durrant- Whyte  during  the  Michaelmas  term  of  1988.  A  number  of  project.*:  arose,  varying 
from  exercises  to  long-term  research  projects.  Much  of  the  work  described' here  is  still  in 
progress,  and  will  be  the  subject  of  theses  and  papers. 

There  are  some  ongoing  activities  at  the  Robotics  Research  Group  of  the  University  of 
Oxford  relating  to  optimal  estimation  that  are  independent  of  the  reading  group  and  which 
are  described  elsewhere. 

1.  Alan  Mrlvor  has  worked  on  characterizing  the  properties  of  the  extended  Kalman 
filter  when  used  to  track  edges  in  a  dynamic  environment  (21]. 

2.  Ron  Daniel1:  and  his  students  are  implementing  very  fast  (800  Hz.)  Kalman  filters 
in  his  work  on  robot  control. 

3.  Guv  Scott  and  Nick  Walker  are  actively  investigating  wave-propagation,  especially 
tlm  Schroedinger  wave  model,  as  an  underlying  mechanist  ‘  mplement  tracking 
and  correspondence  [2G].  This  work  is  described  in  a  SKIDS  •••  r. 


The  work  that  grew  out  of  the  reading  group  is  presented  below  in  the  following  order. 
Bobby  Rao  and  John  Leonard  made  a  brief  survey  of  estimation  techniques.  Barry  Steer 
and  Chris  Brown  implemented  various  examples  of  Kalman  filters:  Steer  applied  the  filter 
to  estimating  the  position  of  a  feature.  Brown  implemented  various  tracking  algorithms 
for  single  targets  with  and  without  clutter  from  [3],  the  text  of  the  reading  group.  Brown 
Implemented  matrix  and  kalman  filtering  utilities  used  by  himself  and  Leonard.  John 
Leonard  is  working  on  a  project  to  develop  and  implement  an  active  sensor  control  strategy 


for  onn  of  the  lab's  mobile  robots.  The  strategy  is  based  on  a  hierarchical  controller 
with  different  level-  of  Attention.  Looking,  and  Recognition  [20],  using  multiple  sensors 
including  vision  and  sonar.  His  contribution  to  this  report  is  on  multi-target  tracking 
for  localization.  Bobby  Rao  has  completed  a  project  to  define  completely  decentralized 
Kalman  filters.  Previous  work  has  relied  on  a  hierarchy  of  distributed  sensors,  but  no  work 
to  date  has  had  a  completely  decentralized  flavor.  The  algorithm  has  been  implemented 
on  a  distributed  computer  network,  and  the  scope  of  the  project  is  growing. 

Huch  Durrant- Whyte  is  actively  using  Kalman  filtering  techniqes  to  do  navigation  and 
tracking  in  a  sonar  application.  He  is  producing  a  paper  on  ’’Navigation  by  correlatinc 
Geometric  sensor  data  ",  and  the  authors  and  editor  of  this  document  owe  much  to  his 

leadership  and  insight. 

2  ESTIMATION  AND  KALMAN  FILTERING 

2.1  Estimation 

2.1.1  Non-Bayesian  Estimation 

Non-Bayesian  Estimation  is  used  when  the  value  being  estimated  is  not  a  random  variable 
but  is  constant.  The  estimate  should  converge  to  this  value  as  the  number  of  readings 
— *  oc. 

Assuming  the  prior  Probability  Density  Function  (PDF)  of  x  is  unknown,  its  posterior 
PDF  is  unavailable,  leading  to  the  use  of  a  likelihood  function: 

-M*)  =  p(  Ajar) 

and  hence  the  Maximum  Likelihood  (ML)  method 

i(k )  =  argmaxxp(zk  |x). 

The  likelihoods  arise  from  empirical  or  analytic  models  of  the  sensor. 


2.1.2  Bayesian  Estimation 


Bayesian  Estimation  is  used  when  the  parameter  to  be  found  is  a  random  variable  (RV) 
with  a  PDF  p(.r).  A  value  of  x  is  assumed  to  have  occurred  via  this  PDF  and  remained 
constant  during  ,ts  measurement  sequence.  The  measurement  sequence  should  converge 
to  the  actual  value  of  x  that  we  are  measuring. 


Given  the  prior  PDF  for  z,  its  posterior  PDF  follows  from  Bayes’  Rule 


p[x 


At 


pA^IApA) 

p(A) 
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I ) t i <-  DM-  :o  *h.e  Maxi;, min  A  1'osteriori  (MAD  method: 

x(k)  =  arpmar  Tp(x\:h )  ~  argwax  T[p{zk\x)p(x  )j. 


Ho* h  t !m  M  1  am  i  MAH  met  hods  yield  mode?  of  a  probability  distribution  (the  most  likely 

vaiue  V 


2.1.3  Least  Squares  Estimation 

The  l.S  method  ic  for  non-random  parameters  and  for  measurements 

z(j)  =  h{j,r)  +  u-(j). 


which  yields 


k 

x(k)  =  argrnirix  ^['0)  -  h(j.  t)]“. 

j=i 


Here  h< . i  is  rise  sensor  model.  The  least  squares  techniques  yields  the  mean,  not  the  mod 
of  tlm  probability  distribution  function. 


O 


2.1.4  Minimum  Mean-Square  Error  Estimation 

The  MMSE  method  is  for  random  parameters  and  yields 

x(k)  =  argmin i-E[(  x  -  t)2^]. 


2.1.5  Linear  Estimation 

The  estimator  is  a  linear  function  of  the  measurement  data. 

x  =  x  +  P TZr„\z  —  -c) 

when' 

rTT  =  E\(x  -  x)(x  -  i)f], 

z  is  the  expected  measurement,  and  (z  -  z)  is  the  tnnouation  produced  by  the  true  mea 
surement. 

2.1.6  Equivalences  Of  Methods 


All  the  above  can  produce  the  same  results  under  certain  conditions: 


•  Ml  =  MAP 

1  his  occurs  when  tfm  prior  PDF  for  x  is  non-informativp.  jp  when  p(x)  =  (  and 
(  — -  0  or  when  the  PDF  is  a  Gaussian  with  n n  — •  oc 

•  IS  =  MI. 

This  occurs  if  the  noises  w(j)  in  z(j)  =  h{j,x)  +  U'(_7)  are  independent  identically 
distributed  (IID)  zero  mean  Gaussian  RVs.  This  also  applies  if  x  is  a  vector  property. 

•  MMSF  =  MAP 

These  coincide  if  the  posterior  PDF  of  x  is  Gaussian  with  arbitrary  mean. 

•  Linear  Estimation 

Wli.  ~  is  r.  Gaussian  RV  the  linear  estimator  of  the  MMSF  form  is  equivalent  to  the 
best  linrnr  estimator  for  arbitrarily  distributed  RVc  with  the  same  first  and  second 
moments.  However  if  the  RVs  are  not  Gaussian  the  MMSE  can  be  a  better  estimator 
than  the  linear  estimator  (ie.  Gaussian  RVs  give  the  worst  case  results  of  an  MMSE 
estimator). 

2.2  The  Kalman  Filter 

2.2.1  Linear  Kalman  Filter  Equations 

Kalman  filtering  is  a  form  of  optimal  estimation  characterized  by  recursive  (i.e.  incre¬ 
mental)  evaluation,  an  internal  model  of  the  dynamics  of  the  system  being  estimated, 
and  a  dynamic  weighting  of  incoming  evidence  with  ongoing  expectation  that  produces 
estimates  of  the  state  of  the  observed  system.  The  basic  Kalman  filter  loop  appears  in 
Fig.  1  ( taken  from  [3]).  Its  input  is  the  system  measurements,  its  apriori  information  is  the 
system  dynamics  and  noise  properties  of  system  and  measurement,  and  its  useful  outputs 
are  the  innovation-[ihe  difference  between  the  predicted  and  observed  measurement,  by 
which  the  filter's  performance  may  be  quantified),  and  the  estimated  system  state. 

Consider  a  system  with  state  vector  x,  moving  under  a  control  u,  affected  by  Gaussian 
noise  v: 


x(*  +  1)  =  F(*)x(*)  +  G(*)u(*)  +  v(*). 


The  system  makes  observations  z  with  Gaussian  noise  w: 


(1) 


z{k)  =  H(k)x(k)  +  w(fc).  (2) 

wherp  v(k)  and  w (k)  are  zero  mean,  white  random  sequences  with  covariances  Q(Jt)  and 
R(*).  respectively. 
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Figure  1:  The  (Linear)  Kalman  Filler  (one  cycle). 


For  such  a  system  the  conventional  Kalman  Filter  equations  for  state  prediction  x( A  4- 1  |  A), 
variance  prediction  P (A  +  1  |  k).  state  update  x(A  +1  |  A  +  1)  and  variance  updalc 
P(A  +  1  |  A-  +  1 ).  where  the  variance  is  defined  as 

P( A  |  A-)  =  £{[x(A)  -  x(A  |  A)][x(A)  -  x(A-  |  A)]T  |  Z*} 

may  he  found  in  (Bar-Shalom  [T] )  and  have  the  following  form: 


Prediction:  The  state  at  the  time  of  the  next  measurement  may  be  predicted: 

x(A  +  1(A)  =  F(A)x(A|A)  +  G(A)u(A).  (3) 

The  state  prediction  covariance  P(A  +  1|A)  is 

P(  A  +  1(A)  =  F(A)P(A|A)Fr(A)  +  Q(A).  (4) 

The  next  measurement  is  predicted  using  characteristics  of  the  measurement  process  and 
the  predicted  state: 

z(A+  1|A)  =  H(A+  l)x(A+  1|A).  (5) 

Update:  The  innovation  v  is  defined  as  the  difference  between  the  predicted  and  actual 
next  measurement: 

i'(fc  +  1)  =  z(A  4-  1)  -  z(A  +  lJA)  (6) 
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Tim  innovation  covariance  S(A-  4-  1  )  is: 


S;A-  +  1)  =  H  (A-  +  1)P(*  +  1|A-)H7(A'  +  ])  +  R(k  +  1).  (7i 

5  can  !io  used  to  compute  the  filter  gain.  W[k  +  1 ): 

W (k+  1)  =  P(A-  +  1  \k  )Ht( k  +  1)S ~'(k+  1).  (8) 

The  innovation  weighted  by  the  filter  gam.  plus  the  predicted  state,  form  the  updated 
state  estimate; 

x(A  +  l|Jt  +  1)  =  x( k  +  l|Ar)  +  W(Jt  +  l)i/(A-  4-  1 ).  (PI 

The  updated  state  covariance  P(A- -F  l|A--fi  1)  is: 

P(A-  +  1|A-  +  1 )  “  P(A-  +  1|A-)  -  \V(A--f  1)S (A-  +  1)W t(A+  1)  nn. 


2.2.2  Extended  Kalman  Filter  Equations 

The  (first  orderl  extended  Kalman  filter  (EKF)  is  a  version  of  the  Kalman  filter  that  dealt 
with  nonlinear  dynamics  or  nonlinear  measurement  equations,  or  both.  It  linearizes  the 
problem  around  the  predicted  state  (a  second-order  EKF  makes  a  second-order  approx¬ 
imation).  The  basic  control  loop  of  Fig.  1  still  applies,  but  measurements  are  predicted 
using  the  nonlinear  measurement  equation  h(.).  The  measurement  model  h[A\x(A)j  is 
linearized  about  the  current  predicted  state  vector,  x(k  -f  1  |  k)  using  the  Jacobian  of  the 
nonlinear  measurement  function  h(  ).  The  calculations  for  filter  gain,  state  update,  and 
covariance  update  use  the  Jacobian  hx(A).  Likewise  state  prediction  is  accomplished  us¬ 
ing  the  nonlinear  state  equation  f(-).  The  plant  model  f(A\x(Ar)]  is  linearized  about  the 
current  estimated  state  vector,  x(k  |  k).  The  state  prediction  covariance  is  computed  using 
the  plant  Jacobian  -fx(A-). 

With  these  definitions  of  f X(A-).  hx(Ar  +  1)  the  extended  Kalman  filter  equations  are  the 
following. 

Prediction: 

x(k  +  1  |  k)  =  f{*,x(/r  |  A))  (11) 

P(*  +  1  I  k)  =  f*(fc)P(fc  |  k)fxT (k)  (12) 

Update: 

S( k  +  1)  =  hx(A-  +  1)P(*  +  1  |  A)hxT(Ar+  1)  +  R(*  +  1)  (13) 

K (fc+  1)  =  P{k  +  1  j  k)hx(k  +  1)S-](A+  1)  (14) 
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XI  A  --  !  I  A  4  11  =  x( A  +  1  I  A)  4-  K(  A  4  1 )  [z(  A  +  1)  -  h[A.  x<  A  ~  1  I  A  i'I 


( 1 M 


Pi  A  -  i  I  A  4  V  =  P(  A  +  1  |  A !  -  \V(  A  1  iSf  A  4  1  )W7  .  A  j-  1 1  l}r., 

In  the  formulation  above  and  in  tlm  example  of  th<-  next  section,  ono  r^.-nrc 

all  measurements  and  makes  the  estimation.  There  has  been  a  considerable  amount  of 
recent  interest  in  t ho  development  of  algorithms  to  process  information  obtained  from 
a  distributed  sensor  system.  This  interest  arises  from  a  need  to  use  parallel  processvnc 
architer  tures  offirientlv.  Parallel  computation  is  needed  if  multi-sensor  systems  are  to  be 
able  to  process  their  data  in  real-time.  A  decentralized  filter  is  presented  below. 


3  CENTRALIZED  FILTERS 


3.1  Anglo-only  Tracking  of  a  Point  Target 


As  an  introduction  to  Kalman  filtering,  we  present  an  extended  Kalman  filter  (1  KF  )  for 
a  system  that  takes  angle-only  measurements  of  a  point  target  in  motion.  An  example  of 
a  measurement  of  this  kind  is  passive  underwater  sonar.  This  scenario  arises  in  robotics 
when  using  infra-red  range  sensors,  which  are  characterized  by  poor  depth  accuracy  but 
which  can  have  excellent  angular  resolution  { 1 1  ]. 


We  consider  a  simplified  two-dimensional  system  with  state  vector  x(A)  =  ref¬ 

erenced  to  a  glohal  stationary  reference  frame.  The  target  moves  forward  in  time  with 
known  heading  angle  o  and  speed  7  subject  to  additive  zero-mean  gaussian  noise  sources 
vT  and  vy: 


T  k  +  1 
.  .Vfc+1 


yk 


+ 


T  cos  6 
_  7  sin 


(17) 


This  is  tlm  plant  equation,  which  we  can  write  as: 


xf  A  +  1 )  =  x(A)  4-  u(  A)  +  v( A).  v(A)  ~  A’(0,  Qfklb  (1^1 


At  time  tk.  a  sensor  located  at  the  origin  takes  an  angular  measurement  z(A)  which  is  the 
target's  angular  position  6k  corrupted  by  zero-mean  gaussian  noise  w(k)  with  standard 
deviation  t n>.  To  relate  the  angle  to  the  state  vector  x(A)  we  define  the  non-linear 
measurement  function  h[x(A)]: 

9k  =  h[A,x(A)]  =  tan-1  — .  (19) 

Tk 

We  can  now  write  the  measurement  equation  for  this  system: 

z(A)  =  h[A,  x(A)]  +  w(A),  w(A)  ~  (0,tr’e).  (20) 
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I  igure  2:  I  racking  a  point  target  with  angle-only  measurements  The  target  moved  from  |ef>  to 
right  with  heading  angle  o  =  -  4a  degree?  and  speed  y  =  0  5  meter?  per  second  Real  (diamond t 
and  e?t  unated  (  oo??  I  target  position?  and  a  prion  confidence  ellipses  are  displayed  even  2  seconds 
The  standard  deviation?  for  plant  ?nnrn>?  t  r  and  t  y  are  R  cm  The  measurement  noise 

standard  deviation  t it  is  1.2  degree?  The  coordinate  axes  are  marked  off  in  1  meter  intervals 

The  above  plant  and  measurement  model?  were  implemented  using  the  Kalman  filtering 
facilities  developed  here  at  Oxford.  Figure  2  shows  a  typical  run.  displaying  the  actual 
target  position  x<k\,  the  a  prion  estimate  x(k  |  k),  and  an  error  ellipse  determined  by 
P( k  |  k).  our  confidence  in  this  estimate.  The  initial  error  ellipse  is  a  large  circle,  reflecting 
little  confidence  in  the  initialization.  Each  new  angle-onlv  measurement  provides  only 
partial  information,  squeezing  the  error  ellipse  in  the  direction  perpendicular  to  8*.  while 
the  error  ellipse  grows  in  the  parallel  direction  due  to  plant  noise. 

3.2  Maneuvering  Targets 

When  targets  mmi> utrr,  i.e.  depart  from  the  basic,  steady-state,  "normal"  dynamic  be¬ 
haviour.  a  tracking  filter  must  respond.  To  the  filter,  maneuvering  is  signaled  by  a  rapid 
increase  in  the  normalized  innovation  Recommended  methods  for  dealing  with  this  situ¬ 
ation  include  (lie  following. 
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1 .  In--  re, i thr  j.r«'r<>x«  muse,  or  certain  component s  of  it .  attributed  to  the  t arg't . 

2.  Fse  several  filter;  ui’h  different  assumptions  in  parallel,  and  combine  their  output1 
probabilMical’y. 

3.  Create  n<v.  filters  „s  needed,  pursuing  a  livpothe?i=  tree  of  parallel  hypotheses  about 
target  state.  'I  hic  tree  must  be  pruned  rapidly  lest  its  maintenance  overwhelm  tin- 
com  put  a  f  ion  a!  resoti  rres. 

4.  Model  nianettvers  as  colored  (correlated)  noise:  in  particular  model  target  accelera¬ 
tion  as  a  zero  mean,  first-order  Markov  process  (one  with  exponential  an tororreln- 
ti-  1 ! :  !. 

r>.  perform  v'-p'.it  (stiinatiofi.  in  which  measurements  based  on  the  nonmaneuverinc 
model  are  used  to  detect  and  estimate  the  control  input  applied  to  the  plant  dynam¬ 
ics.  and  that  control  input  in  turn  is  used  to  correct  the  state  estimate. 

fi.  Pc.-*  vmuthlr  dvnmzion  filtering.  in  which  the  maneuver  is  considered  part  of  the 
plant  dynamics,  not  noise.  Maneuver  detection  causes  the  substitution  of  a  diffc  iej,t . 
higher  -  order  dvnam'c  mode]  for  the  lower-order,  "quiescent"  model. 

liar  Si;  a  be:  i  compares  the  performance  on  maneuvering  target;  of  three  filters;  two 
level  u  f  i-e  t.oise.  variable  dimension  ( VD)  filtering,  and  input  estimation  (IF.),  lie  notes 
that  the  computational  effort  ratios  between  the  three  are  1:2:8.  His  study  reveals  that 
the  t w . v  level  white  noise  filter  doe;  surprisingly  well,  being  slightly  worse  than  the  VI) 
filter  but  definitely  better  than  the  IP  filter. 

\\>  chose  to  implement  the  VD  filter,  in  the  light  of  its  relatively  low  computational 
cost  and  relatively  high  efficacy  in  the  Bar-Shalom  study.  Our  illustrative  application 
was  to  target  moving  in  two  dimensions  at  constant  velocity  until  some  time  at  which 
it  begins  constant  acceleration  in  the  same  direction.  The  quiescent  filter  is  simply  the 
constant- velocity  target  filter.  thn  maneuvering  filter  is  for  a  constant  acceleration  target. 

Fig.  3' a)  shows  tlm  normalized  innovation  (i.e.  error  measure)  of  the  constant- velocity 
filter  as  time  passes.  Performance  starts  degrading  at  7  =  5.  when  acce!erafion  begins. 
Fig.  31  b)  shows  the  corresponding  estimate  of  y  position,  which  gradually  degrades  through 
time.  The  YJ)  filter  has  remarkably  better  performance,  which  underlines  the  importance 
of  accurate  dynamic  modeling.  Fig.  3(c)  shows  the  normalized  innovation  of  the  YD  filter 
(note  t)]P  scale  change).  The  maneuvering  filter  was  switched  in  at  T  —  6.  Fig.  3(d)  shows 
the  corresponding  estimate  of  y  position. 

3.3  Targets  in  Noise 

Tracking  an  object  in  the  presence  of  spurious  measurements  ( clutter )  can  be  done  in 
several  ways.  All  assume  a  validation  gate  outside  of  which  measurements  are  ignored:  its 
size  is  a  function  of  the  desired  probability  of  including  the  true  measurement,  and  can  be 
derived  from  a  chi  squared  calculation  applied  to  the  normalized  innovation. 


Figure  3:  Performance  of  (ho  Variable  Dimension  filter,  (a)  Estimated  y  position  for  target  that 
start*  mnn<'u\ f'nnp  at  7=5  (M  Normalized  innovation  of  quiescent  filt <=r  applied  to  target  (c) 
Estimated  y  position  tor  target  from  VD  filter  (d)  Normalized  innovation  of  VD  filter  (note  scale 
change  compared  to  ( b ) ) . 
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Figure  4:  Real  (diamond)  and  estimated  (cross)  target  (racks,  and  elliptical  validation  gate, 
through  tim<~.  showing  loss  and  rea'-quisit  ion  of  track 

1.  The  optimal  way  to  track  a  sin  cl"  target  in  clutter  is  the  track-splitting  approach, 
in  which  a  tree  of  possible  tracks  is  maintained.  This  can  be  a  combinatorialiy 
expensive  method . 

2.  An  obvious  alternative  to  treating  all  measurements,  as  it  were,  in  parallel,  is  to  pick 
a  single  candidate  measurement  and  proceed  as  if  if  were  the  right  one.  The  obvious 
candidate  is  tlm  one  closest  in  measurement  space  (that  one  with  smallest  normalized 
innovation),  so  this  technique  is  called  the  nearest-neighbor  standard  filter  (NNSF). 
The  problem  is  that  the  true  measurement  can  be  missed. 

3.  A  third  approach  is  the  probabilistic  data  association  filter ,  or  PDAF.  In  it,  the 
measurements  within  the  validation  gate  are  probabilistically  blended  to  yield  a 
combined  innovation  which  is  input  into  the  Kalman  filtering  process.  The  problem 
is  that  the  result  does  not  correspond  to  that  of  any  actual  measurement. 

The  application  illustrated  in  Figure  4  is  tracking  a  constant-velocity  target  moving  in 
two  dimensions.  The  plant  and  measurement  are  both  noisy,  with  the  measurement  noise 
being  drawn  from  a  contaminated  Gaussian,  in  which  with  some  probability  the  measure¬ 
ment  noise  has  different  parameters  (here,  higher  variance)  than  the  noise  expected  by 
the  filter.  The  filter  produces  a  validation  gate,  based  on  the  innovation  covariance.  A 
measurement  within  the  validation  gate  is  used,  while  one  outside  the  gate  is  ignored.  In 
the  run  illustrated,  the  elliptical  validation  gate  (here  a  circle)  shrinks  while  good  mea¬ 
surements  are  obtained,  and  grows  when  a  measurement  is  missed  —  this  adjustment 
makes  reacquisition  more  likely.  The  figure  shows  snapshots  of  the  validation  gate  during 
a  serious  loss  and  reacquisition  of  track. 
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General  aspects  of  the  behaviour  of  the  NNSF  and  FDAF  filters  may  be  predictable 
on  abstract  grounds.  For  instance  we  might  make  the  following  predictions  for  uniformly 
distributed  clutter  and  high  probability  of  detrctinn  ( probability  that  the  target  i?  detected 
at  all.  either  inside  or  outside  the  validation  gate.) 

].  With  both  the  NNsr  and  PDAT  filters  tracking  in  clutter,  as  time  goes  on  it  is 
increasingly  likely  that  the  filter  will  "lose  track",  e.g.  start  tracking  clutter,  or  have 
an  estimate  of  target  state  outside  some  fixed  bound. 

2.  With  a  "non-maneuvering”  NNSF  filter,  low  and  high  clutter  levels  may  be  less 
immediately  harmful  than  medium  levels,  since  with  low  clutter  the  target  is  likely 
to  be  nearest  the  predicted  state,  and  with  high  clutter  there  is  likely  to  be  a  clutter 
point  near  the  predicted  state.  It  would  seem  that  at  intermediate  level  the  clutter 
would  be  more  likely  to  attract  the  filter  away  from  the  target. 

3.  The  NNSF  filter  would  seem  more  likely  to  make  serious  errors  bv  tracking  clutter 
since  ii  does  not  weigh  the  evidence.  The  performance  of  the  TDAF  should  degrade 
more  gracefully  as  conditions  get  worse. 


We  implemented  the  NNSF  and  FDAF  filters,  and  used  them  to  provide  individual  out- 
put  tracks,  as  in  the  previous  work.  Also  the  programs  were  embedded  in  Monte  Carlo 
simulations  to  provide  data  over  a  number  of  runs  in  statistically  similar  situations.  Tim 
results  confirm  the  above  expectations  but  also  provide  some  surprises.  Fig.  5  shows  indi¬ 
vidual  tracking  runs  for  situations  with  uniformly  distributed  clutter  of  increasing  density. 
The  number  of  clutter  points  in  the  validation  gate  was  determined  by  rounding  a  nor¬ 
mal  variate  of  indicated  mean  (the  clutter  density)  and  standard  deviation  to  the  nearest 
integer,  and  uniformly  distributing  the  resulting  number  of  clutter  points  throughout  the 
validation  gate  volume.  In  these  runs  the  volume  was  close  to  unity.  Thp  NNSF  figures 
should  be  compared  with  Fig.  6,  which  shows  FDAF  results  for  similar  situations. 

Figs.  5  and  6  illustrate  indeed  that  lower  clutter  levels  can  result  in  worse  NNSF  perfor¬ 
mance  than  higher  levels,  and  that  performance  of  both  filters  falls  off  as  time  increases. 
They  perhaps  furnish  a  mild  surprise,  viz.  the  ability  of  the  PDAF  to  reacquire  the  track. 
This  happens  for  lower  clutter  levels,  and  presumably  occurs  when  the  "signal  to  noise" 
ratio  is  high  in  the  validation  gate  (e.g.  there  are  few  measurements  in  the  validation  gate, 
and  at  least  one  of  them  is  near  the  actual  position  of  the  target  and  correctly  is  accorded 
high  weight).  The  behavior  of  the  FDAF  in  high  clutter  conditions  is  not  as  surprising  — 
it  drifts,  taking  the  average  of  the  random  clutter. 

Fig.  7  shows  statistics  gathered  over  N  =  DO  runs  with  the  NNSF  filter.  It  should  be 
compared  to  Fig.  8.  The  plots  show  the  fraction  of  lost  tracks  and  the  average  final  error 
of  the  filter's  estimate.  Both  functions  vary  over  the  set  of  tracking  times  {4.  8,  16,  32  } 
timesteps,  and  both  vary  over  the  set  of  clutter  densities  {  0.25,  0.5,  1.0,  1.5,  2.0,  3.0  }. 
Here  "lost  track"  is  defined  as  the  estimated  position  being  more  than  some  fixed  distance 
(here  2.0)  from  the  actual  position  of  the  target.  Thus  it  is  possible  to  imagine  the  filter 
actually  tracking  clutter  for  the  entire  run,  going  wildly  w/rong  in  its  estimates,  but  luckily 
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Figure  5:  In  (his  figure  and  (he  nex(.  (he  filter  is  a  const  ant- velocity  (linear)  Kalman  filter.  The 
plant  noise  is  an  acceleration  component  (white  noise  of  mean  0.0  and  variance  q  =  0.2  )  Clutter 
density  <r  =  0.4.  (Parallelepidal)  validation  gate  size  is  such  that  .99  of  target  measurements  should 
fall  within  it  initially  The  measurement  noise  has  variance  0.2  for  the  r,  0  1  for  y  (a)  Error  in  X 
vs  time  in  NNSF,  tracking  situation  parameters  as  in  text,  clutter  density  0.5.  (b)  Error  in  Y  vs 
time  in  NNSF,  clutter  density  1.0.  (c)  Error  in  X  vs  time  in  NNSF,  clutter  density  2.0 
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Figure  6:  (a)  Error  in  X  vs  ( ini'*  in  PDAF,  tracking  situation  parameters  as  in  text,  clutter  density 
0  5  (b)  Error  in  Y  vs  time  in  PDAF,  clutter  density  1.0.  (c)  Error  in  X  vs  time  in  PDAF.  clutter 
density  2  0. 


Figure  7:  This  figure  and  the  next  show  the  fraction  of  lost  tracks  (a)  and  the  average  final  error 
fb)  of  the  filters  estimate.  Both  functions  vary  over  the  set  of  tracking  times  {4,  8,  16,  32  } 
timesteps  (axis  T),  and  both  vary  over  the  set  of  clutter  densities  {  0.25,  0.5,  1.0,  1.5,  2  0,  3.0  } 
(axis  D)  Lost  track  means  the  estimated  position  is  more  than  some  fixed  distance  from  the 
actual  position  of  the  target.  This  figure  shows  results  for  the  NNSF,  with  tracking  situation  as  in 
previous  figures. 
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Figure  8:  Lost  tracks  (b)  and  average  final  error  (b)  vs  track  length  and  clutter  density  PDAF. 
with  tracking  situation  as  in  last  figure. 


arriving  inside  the  threshold  distance  just  at  the  last  step,  and  thus  not  "losing  track"  by 
this  definition. 

The  average  final  error  function  is  meant  to  quantify  the  filter  performance  more  than 
the  discrete  "lost  track"  measure,  illustrating  a  linear  loss  function  corresponding  to  the 
intuition  that  an  estimate  closer  to  the  truth  at  the  final  timestep  is  better.  Result?  like 
those  in  Figures  5  and  6,  we  are  probably  safe  in  inferring  that  a  closer  final  estimate  also 
betokens  a  better  estimate  throughout  the  run. 

The  plots  are  perhaps  surprising  in  that  by  this  definition  of  lost  track,  the  NNSF  outper¬ 
forms  the  PDAF  fairly  convincingly  over  a  large  range.  These  results  are  typical  of  many 
we  obtained,  but  it  is  occasionally  possible  to  engineer  situations  where  the  PDAF  loses 
track  less  often.  At  least  it  seems  fair  to  say  that  the  situation  is  more  complicated  than 
it  appears  from  Figure  6.1  of  [3],  which  indicates  a  marked  superiority  of  PDAF  along 
axes  whose  semanti'cs  are  not  clear  from  the  text  (perhaps  the  original  paper  gives  more 
details). 

The  plots  are  perhaps  not  surprising  in  that  they  accord  with  the  prediction  of  graceful 
degradation  of  the  PDAF  in  terms  of  the  average  error  metric,  using  which  it  convincingly 
outperforms  the  NNSF.  The  higher  sensitivity  of  NNSF  to  intermediate  clutter  levels  is 
again  demonstrated. 


3.4  Navigation  by  Multi-target  Tracking 

We  are  using  a  Kalman-filter-based,  multi-target  tracking  approach  to  mobile  robot  navi¬ 
gation.  The  goal  is  to  get  optimal  performance  with  a  single  sensor  through  a  strategy  of 
Active  Sensor  Control.  Ultimately,  we  shall  use  a  hierarchical  control  strategy  with  three 
layers  (attention,  looking,  recognition).  In  the  limited  navigational  task  we  undertake  first 
(maneuvering  in  a  known  laboratory  with  unknown  obstacles),  the  recognition  aspect  is 
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subsumed  into  ilm  looking  aspect. 


The  navigation  problem  can  he  divided  into  the  two  distinct  tasks  of  localization  (deter¬ 
mining  the  absolute  position  of  the  robot),  and  obstacle  avoidance.  In  the  multi  target 
tracking  framework,  localization  is  a  process  of  looking  for  expected  events;  the  sensors  are 
directed  to  instantiate  the  locations  of  beacons,  which  are  reliable,  easy-to-sense.  natural 
features  of  the  robot's  environment,  such  as  the  walls  of  the  room.  Precise  localization  of 
position  can  then  be  obtained  by  using  a  target-based  [3]  approach  to  track  these  beacons 
as  the  robot  moves.  The  task  of  obstacle  avoidance  is  achieved  by  a  measurement-based 
[2o]  tracking  technique  whose  attention  is  captured  by  unexplained  events  in  the  path 
of  the  robo*  We  show  the  localization  component  of  this  navigation  technique  using  a 
single  sonar  sensor  mounted  on  a  simple  mobile  robot  with  point  dynamics.  Although  the 
scheme  ha:  been  run  on  the  robot  hardware,  the  illustrations  come  from  a  simulator  we 
are  developing  that  will  incorporate  detailed  sensor  and  environment  models  for  sonar. 

Ultimately  this  work  will  incorporate  both  the  "top-down"  and  "bottom-up"  control  styles 
that  can  be  found  in  perception  (see  the  Discussion  section).  In  the  full  implementation, 
the  rernaruhon  layer  is  complex,  involving  path-planning,  obstacle  avoidance,  and  model, 
learning  and  maintenance  —  slow  processes  of  a  global  nature.  The  attention  layer  provides 
a  high-speed  interface  to  the  real  world. 


3.4.1  The  System  Modei 

The  system  dynamics  for  one  of  Oxford's  mobile  vehicles  are  illustrated  in  Fig.  9(a).  The 
state  of  the  vehicle  at  time  tk  is  described  by  the  state  vector  x(k)  =  (?* ,  yk , 6k- Tk ).  Xk 
and  yk  are  the  x  and  y  coordinates  of  the  center  of  the  vehicle  referenced  to  a  global  frame. 
Oh.  is  tlm  orientation  of  the  vehicle  referenced  to  a  global  frame.  Tk  is  the  speel  of  the 
vehicle  at  time  tk.  which  represents  the  linear  distance  the  vehicle  will  travel  during  the 
time  interval  from  tk  to  tk±\-  The  motion  of  vehicle  is  such  that  during  each  timestep  it 
travel®  first  in  a  straight  line  forward  from  position  (xk,yk)  to  position  {xk+\ .  yk+ 1  )■  then 
rotates  to  its  final  orientation  6k+i  ■ 

Xk+ 1  =  Xk  +  Tk  cos  6k 
Vk+t  =  Vk  x  Tk  sin  6k 

The  robot  motion  is  controlled  by  specifying  a  control  input  u (k)  =  (0.0,  A6k-  Al\  )r. 
A 6k  controls  the  final  rotation  applied  after  the  linear  movement  forward  for  the  current 
time-step.  This  rotation  is  subject  to  a  random  disturbance  modeled  as  zero-mean 
Gaussian  with  variance  q^,\ 

6k+ 1  =  6k  +  A 6k  +  Ujh  ~  AT(0,  q<t) 

ATk  controls  the  distance  traveled  forward  for  the  next  time-step,  which  is  subject  to  a 
random  disturbance  v t,  modeled  as  zero-mean  Gaussian  with  variance  qj: 

Tk+i  =  ATk  +  t-’Ti  t?7  ~  A  (0 ,qj) 
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Tigur^  9:  (a)  System  dynamics  for  robot  vehicle,  (b)  Taking  a  range  measurement  from  a  wall 
( j- 1 .  tn ,  Ok  1  ic  the  position  of  the  robot  vehicle  in  global  coordinates  (/?,.  $,  )  represents  a  line  in 
global  coordinates.  is  a  noisy  measurement  of  the  perpendicular  distance  from  the  vehicle  to 
w  all  p, . 


The  overall  plant  equation  of  the  system  is 


•*T+1 

'  xk  +  Tk  cosd>/t ' 

-  0  ' 

'  0  ' 

Vk  + 1 
<t>k  +  1 

= 

yk  +  Tk  sin  <bk 
<t>k 

+ 

0 

A  6k 

+ 

0 

-T  k+  1  - 

0 

-A  Tk\ 

-  *7  - 

which  may  he  written  as: 

x{k  +  1)  =  f[fc.x(k)]  +  u(fc)  +  v(A-),  v(k)  ~  A'(0,Q(k)) 
where  f[T\x(b)j  is  a  non-linear  function  of  the  system  state. 


(21) 


(22) 


3.4.2  The  Measurement  Model 

The  measurement  model  is  for  a  stationary  robot  vehicle  taking  range  measurements  to  a 
number  of  walls  using  a  sonar  sensor.  For  now,  we  assume  perfect  measurements  of  4>k,  the 
orientation  of  the  vehicle.  (Our  system  has  a  9-bit  resolution  digital  compass  that  gives 
us  this  information.  We  also  have  developed  reliable  algorithms  to  extract  orientation 
information  directly  from  the  sonar  data).  The  model  of  the  environment  is  a  list  of  n  line 
segments  that  represent  walls  and  large  planar  objects  in  the  room.  A  line  segment  p,  is 
represented  by  R,,  its  perpendicular  distance  from  the  origin,  and  <?,,  its  orientation  with 
respect  to  the  origin.  In  terms  of  these  parameters,  the  equation  of  the  line  is: 

Ri  -  rcos0,  -  ysinfl,  =  0  (23) 
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The  sensor  returns  the  minimum  distance  of  any  object  delected  in  its  30  deerer  beam 
width.  Thus  if  it  is  pointed  in  a  direction  approximately  perpendicular  to  the  wall,  the 
range  value  obtained  is  the  perpendicular  distance  from  the  vehicle  to  the  wall.  The  active 
sensor  control  framework  nsec  an  a  priori  estimate  of  position  x(k  \  k  —  ])  to  direct  the 
senior  to  look  nearly  perpendicular  to  a  given  wall  p,  =  (/?,,#,)  to  obtain  a  measurement 
c,  of  the  perpendicular  distance  to  the  wall  (Fig.  9(b)). 

Let  r,  denote  the  true  perpendicular  distance  from  a  given  vehicle  location  (xk.yk)  to  the 
wall.  Then 

r,  =  |7?t  -  xjtcostf,  -  ytsin0,|.  (24) 

The  absolute  value  arises  since  the  vehicle  could  be  on  either  side  of  the  wall.  The  mea¬ 
surement  2,  is  the  true  perpendicular  distance  r,  corrupted  by  zero-mean  Gaussian  noise 
it'  with  variance  ir.: 

z,  =  r,  -f  u\  tr  ~  A'(O.ttv) 

Suppose  that  at  time  tk  the  robot  is  predicted  to  he  in  the  location  x{k  |  k  -  ]).  The 
predicted  location  is  used  to  suggest  ??  walls  to  obtain  range  readings  from.  The  sensor 
is  directed  to  look  for  each  of  these  walls,  and  the  returned  values  are  checked  with  an 
appropriate  validation  gate  to  yield  validated  range  readings  2,  for  m  of  the  v  visible  walls. 
We  combine  the  validated  range  measurements  to  form  a  composite  measurement  vector 

»(*>  =  ki . ^lr-  To  relate  this  measurement  vector  z (k)  to  the  vehicle  position  x(k). 

we  define  the  non-linear  function  h 

|/?i  -  x k  co$0\  -  yk  sin  #i  | 

h[*.x(*)]=  .  (25) 

.  I Rm  -  xkcos8m  -  yk  sin  6m\ . 

Finally,  a  composite  noise  vector  w (k)  completes  the  measurement  model: 

z(k)  =  h[fc,x(fc)]  +  w(fc),  w(fc)  ~  Ar(0,R(fc))  (26) 

The  function  h[<r.x(fr)]  incorporates  the  prior  information  concerning  the  walls  p,  for 
which  validated  range  readings  have  been  obtained  at  this  time  interval.  So  far  we  have 
ignored  the  data  association  problem,  assuming  that  each  range  measurement  comes  from 
the  appropriate  wall.  The  viability  of  this  assumption  depends  on  how  accurately  the 
system  model  predicts  the  vehicle's  movement;  experimentally  it  has  been  justified  so  far. 

3.4.3  Extended  Kalman  Filter  Equations 

The  extended  Kalman  filter  equations  for  the  plant  and  measurement  models  described 
above  in  equations  22  and  26  use  linearized  plant  and  measurement  models.  The  plant 
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model  f U'.x'T'l]  is  linearized  about  the  current  estimated  state  vector,  x( k  |  k\.  We  ac¬ 
complish  this  by  defining  the  plant  Jacobian.  fx(k): 


fx(H  =  [Vxf  TU\x)] 


T 

x  =  x  ( JL- 1 »  ) 


'1  0  —  sin  cosdk 

0  1  1\  cos  d>k  sind>k 

0  0  1  0 

.0  0  0  0 


x=x(k\k) 


(27) 


The  measurement  model  h[J\x(/;)J  is  linearized  about  the  current  predicted  state  vector, 
x(k  -f  1  |  k).  We  define  the  measurement  Jacobian  hx(lc  +  1)  to  be: 

f  ±  cos  6\  ±  sin  8\  0  0 1 


hx(k  +  1 )  -  [ ^ x h T ( /c ,  x)]x=X(k+i|fc)  - 


±cos(?m  isintfn  0  0 


(2.5) 


The  ±  signs  arise  because  h!A\  xf  k )]  contains  the  absolute  value  function  for  reasons  given 
above.  In  calculating  f x(k).  the  appropriate  sign  for  each  wall  is  chosen  based  on  the 
dead- reckoning  estimate  of  the  robot's  position.  The  absolute  value  functions  would  make 
the  Jacobian  of  h[F.x(F'l]  unstable  at  sensing  locations  very  close  to  the  wall.  This  i= 
not  a  problem  in  practice  because  the  sensor  is  located  at  the  center  of  the  vehicle.  The 
definitions  of  fx( k)  and  hx(/c  +  1 )  are  used  in  the  extended  Kalman  filter  equations  (11) 
-  ( I G )  in  a  multi-target  control  structure. 


3.4.4  Results 

We  avoid  some  of  the  filter  initialization  difficulties  by  starting  the  vehicle  from  an  exactly- 
known  initial  location  and  setting  the  initial  state  covariance  matrix  P(0  |  0)  to  zero. 
Because  the  motion  of  the  robot  vehicle  has  a  significant  unpredictable  component,  the 
state  covariance  matrix  P (k  |  k)  grows  considerably  with  time  if  no  updates  of  position 
from  the  sensor  are  provided. 

Four  runs  of  the  simulated  system  are  shown  in  Fig.  10.  The  error  ellipse  is  defined  by 
the  first  four  elements  of  P (k  |  k),  and  represents  the  uncertainty  in  the  prediction  of  the 
vehicle  position  (r^.t/fc).  In  Fig.  10(a),  the  speed  noise  vp  causes  the  uncertainty  ellipse 
to  grow  with  time  in  the  direction  of  motion  of  the  vehicle.  The  heading  angle  noise 
t'4,  causes  the  uncertainty  ellipse  to  grow  in  the  direction  perpendicular  to  the  vehicle's 
motion.  Our  task  for  the  measurement  process  is  to  decrease  the  size  of  this  ellipse,  and 
hence  increase  the  confidence  in  the  state  estimate  x(k  |  k).  Fig.  10(b)  and  (c)  show  the 
effects  of  measurements  from  one  and  two  walls,  respectively.  Fig.  10(d)  shows  a  run  in  a 
complex  environment  with  many  walls.  The  sensor  is  directed  to  take  measurements  from 
alJ  visible  walls. 
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Figure  10:  (a)  A  run  with  no  validated  measurement?  The  triangle  represents  the  actual  vehicle 
position  and  orientation  (t*.  yk.  <bk)>  the  rectangle  represents  the  estimated  vehicle  position  and 
orientation,  and  the  ellipse  represents  the  uncertainty  in  the  estimates  of  xk  and  yt.  (b)  A  run 
taking  range  measurements  from  a  single  wall.  The  error  ellipse  shrinks  perpendicular  to  the  wall 
as  a  posteriori  confidence  in  the  estimate  of  rt  and  yt  increases  with  measurements.  The  wall 
comes  into  view  and  disappears  from  view  during  the  run,  causing  large  effects  in  the  error  ellipse, 
(c)  Measurements  from  two  walls.  Information  from  two  directions  reduces  error,  (d)  A  run  in  a 
complex  environment  using  measurements  from  all  walls  visible  from  a  given  location 


figure  11:  Choosing  the  bf-st  Sensing  target  for  (a)  simplified  two-wall  environment  and  (h) 
multiple-wall  environment  All  visible  sensing  targets  are  evaluated  to  choose  th"  target  that 
provides  the  most  useful  information  based  on  the  a  priori  estimate  of  and  yk .  The  sensor  then 
looks  for  this  "best  "  wall  to  update  the  vehicle  position 

3.4.5  Choosing  the  Best  Sensing  Target 

Since  range  measurements  from  a  single  wall  provide  only  partial  information,  a  wall-based 
sonar  localization  algorithm  needs  to  take  measurements  from  several  different  directions, 
as  illustrated  in  the  examples  above.  However,  it  is  undesirable  in  many  cases  to  make 
the  vehicle  stop  to  take  observations  of  several  different  walls  from  the  same  location.  1  o 
give  the  vehicle  a  dynamic,  “on-the-fly'  localization  capability  we  advocate  a  strategv  of 
controlling  the  sensor  to  track  different  walls  successively  as  the  vehicle  moves. 

Our  ta^k  is  to  keep  the  state  estimate  covariance  P (k  \  k)  as  small  as  possible.  This 
corresponds  to  keeping  the  error  ellipse  determined  bv  P(k  |  k)  from  growing  too  large 
in  any  one  direction.  Using  this  criterion,  the  best  sensing  target  is  the  wall  most  nearlv 
perpendicular  to  the  largest  eigenvector  of  P{k  |  k).  Fig.  11  shows  runs  in  two-wall 
and  multiple- wall  environments  that  follow  this  control  policy  of  choosing  the  best  wall. 
This  strategy  provides  a  means  of  localizing  position  “on-the-fly”  with  a  single  sensor  by 
focusing  attention  on  different  walls  that  come  in  and  out  of  view  as  the  robot  moves. 


3.4.6  Future  Work 


In  this  implementation.  rising  a  single  Kalman  filter  to  derive  the  position  of  the  robot  is 
effective  because 


1.  The  environment  lias  been  known  exactly  a  priori 

2.  The  environment  has  no  moving  objects  and  no  clutter. 

In  a  practical  situation,  an  ultrasonic  navigation  system  may  need  to  deal  with  an  unknown 
environment  that  contains  unmodeled  walls,  unknown  moving  objects,  and  clutter.  For 
this  reason,  in  addition  to  tlm  central  Kalman  filter  for  vehicle  position,  a  multi-target 
framework  that  initializes  and  maintains  tracks  for  individual  targets  in  the  environment 
is  necessary.  We  plan  to  implement  the  three-layer  strategy  of  Active  Sensor  Control  as  a 
mechanism  for  managing  the  multi-target  tracking  approach  to  navigation.  In  the  sensor 
control  framework,  we  shall  develop  a  set  of  local  attention  processes,  each  tracking  a 
particular  target  or  searching  for  new  targets,  and  carrying  out  some  fast,  local  processing 
to  facilitate  this  local  control,  Thp  looking  layer  will  perform  the  multi-target  tracking 
per  ,sc.  assigning  local  tracking  resources  to  follow  individual  pre-identified  targets  and 
responding  to  new  target1:  and  unexplained  events. 


4  A  DECENTRALIZED  FILTER 

4.1  Fully  Decentralized  Decision  Making 

Previous  attempts  at  decentralized  Kalman  filtering  have  eithpr  assumed  a  completely 
centralized  architecture  ( Fig  1)  in  which  all  the  sensor*'  observations  are  passed  hack  to 
a  central  processing  facility  that  fuses  the  data.  [3],  [8],  [7],  or  assumed  some  level  of  local 
embedded  processing  capability  in  each  sensing  node,  but  still  however  reiving  on  a  cen¬ 
tra!  processing  facility  to  perform  the  global  data  fusion  (ie.  hierarchical  decentralization) 
[16].  [17],  Since  both  of  these  methods  require  a  single  central  facility  to  perform  overall 
data  fusion  they  suffer  from  the  associated  problems:  potential  computational  bottleneck' 
and  the  susceptibility  to  total  system  failure  if  the  central  facility  should  fail.  Harris  and 
White  [IS]  give  descriptions  of  algorithms  for  decentralized  system  architectures  using 
blackboards  for  communication  between  nodes,  but  they  suffer  from  the  well-known  prob¬ 
lems  associated  with  blackboard  systems  (such  as  keeping  the  blackboard  current  and  free 
of  redundant  information!. 

Fully  decentralized  derision  making  [13]  is  advantageous  to  hierarchically  decentralized 
and  other  forms  "*f  distributed  system  architectures  because: 

1.  It  allows  complete  parallelization  of  any  algorithm.  Therefore  the  system  would 
he  ideal  for  implementation  on  a  parallel  processing  network  (such  as  a  trans¬ 
puter  array). 
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Figure  12:  Algorithm  for  individual  nodes 

2.  It  lends  to  a  speed  increase  over  other  architectures  by  removing  potential 
computational  bottlenecks. 

.1.  It  give?  a  system  that  is  very  resilient  to  loss  of  one  or  more  of  its  nodes  (ie.  a 
highly  furwahlr  system). 

Distribute.!  coming  and  algorithms  for  sensor  fusion  that  can  be  distributed  amongst 
several  cen-inc  node®  have  interesting  applications  in  robotics,  process  plants  and  C? I 
field®.  1  he  algorithm  we  present  here  is  fully  decentralized,  requiring  no  cen tral  processing 
f <i ■'  i  1  i r y  to  pe[f..rIts  da*a  fusion  (24].  The  information  communicated  between  nodes  is 
simple  and  the  equations  for  assimilation  of  other  nodes'  data  are  no  more  complex  than 
those  for  a  conventional  Kalman  Filter.  Our  filter  reaches  the  same  global  optimum  as 
a  conventional  Kalman  Filter,  but  since  this  filter  may  be  run  simultaneously  at  the  m 
sensing  nodes  of  an  m  node  system  it  performs  faster. 


4.1.1  The  Decentralizing  Algorithm 

Tic.  12  shows  the  operations  performed  by  each  one  of  the  nodes  in  the  svs  em.  Each 
node  is  initialized  with  estimates  of  the  state  of  the  system  together  with  an  associated 
variance  of  that  estimate.  These  initial  values  for  state  estimates  and  variance?  may  be 
obtained  in  several  ways  and  the  methods  used  by  the  implementations  given  in  this  paper 
are  outlined  in  later  sections.  After  initialization,  the  main  loop  begins  with  each  node 
taking  a  reading  from  its  sensor  of  the  state  of  the  system.  With  this  reading  (and  its 
associated  variance)  the  node  is  able  to  compute  conventional  Kalman  Filter  equation® 
to  reach  its  own.  new  estimate  0f  the  state.  Each  node  then  broadcasts  this  estimate  to 
the  otlmr  nodes  and  receives  information  being  broadcast  to  it.  Last,  each  node  compules 
assimilation  equations  to  take  into  account  the  data  it  has  just  received;  in  fact,  each  node 
thus  locally  computes  a  global  estimate. 

This  paper  includes  details  of  a  linear  version  of  the  algorithm  in  which  each  node  shares 
the  same  coordinate  frame  and  has  the  same  representation  of  the  state  of  the  system  as 
other  nodes  In  the  nonlinear  case,  each  node,  although  measuring  the  same  state,  is  in 
its  own  coordinate  frame  and  uses  its  own  representation  of  the  state  which  may  or  may 
not  coincide  with  the  representation  used  by  other  nodes.  This  case  is  treated  in  full  in 
(24]  and  i®  riot  derived  here,  although  experimental  results  are  given. 
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4.1.2  The  Linear  Algorithm 

Consider  a  system  with  state  verier  x.  taking  observations  z  with  Gaussian  noise  sour  re5 
w  and  v  respectively: 


xU-  4-  1)  =  F (A-)x(A-)  +  G(it)w(*) 


(29) 


z(  k)  =  H(  k)x(k)  +  v(A) 


(30) 


where 


V 


\v(b  ) 
v  ( k ) 


wr(;).  vT(» 


Q(A-)  Cr A-)  \ 
CT{k)R(k)  j 


Often  it  is  true  that  C  (the  covariance  between  plant  and  measurement  noise)  is  zero. 

For  stir h  a  system  the  conventional  Kalman  Filter  equations  for  state  prediction  xf k+  1  ]  k  ). 
variance  prediction  P(A-  4-  1  |  k),  state  update  x{k  -f  1  |  k  +  1 )  and  variance  update 
P(A-  +  1  |  k  -r  1 ).  where  the  variance  is  defined  as 

P(*  I  k)  =  E{[x(k)  -  x(k  (  k)][x(k)  -  x(k  (  *}]T  (  Zk} 

may  he  found  in  (I)ar-Shalom  [3])  and  have  the  following  form: 

Prediction; 

x{k  +  1  |  *•)  =  Fx(Jt  |  k)  +  GCR-'zti-)  (32) 

P(*  4  1  |  k)  =  FP(F  |  k)tr  +  GQG7'  (33) 

Update: 

x(*  4  1  \k  +  1)  =  [I  -  WH]x(fc  +  1  |  it)  +  W z(Jt  +  1)  (34) 

P~'(k  +  1  |  k  +  1)  =  p-*(it+  1  |  k)  +  HrR-’H  (35) 

where 

W  =  P(A-  +  1  |  it  +  l)HrR-’  (3G) 
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(37) 


F  =  F  -  GCR-'H 

To  decentralize  them  wo  must  make  the  following  Assumption  (from  Ha.shemipour  [1F>] ): 
Assume  we  ran  partition  tlm  observation  vector  into  m  subvectors  of  dimension  »»,.  As¬ 
sume  also  that  we  ran  partior.  the  H  matrix  conformably.  (This  means  a  node  cannot 
make  a  full-rank  observation  of  the  system  state.)  We  can  therefore  say  that 

v,(A-|  =  [v,rf*)....v*  (*)]T 

and  t  hat 

E{v(fr)vr(fr)}  =  blocKd,ag{RAk) . R  ,„(*)} 

H(k)  =  {Hl(k) H  TJk)) 

*(*')  =  . ^m(M} 

Front  this  assumption  we  can  also  partition  the  F  and  G  matrices  and  also  the  state 
estimates  aiKj  variances,  x  and  P.  This  allows  each  node  to  implement  its  own  local 
Kalman  Filter  for  its  own  local  estimate  of  the  state. 

Each  node.  ?  ha=  a  system  model  and  takes  observations  that  are  individualized  versions 
of  eq.  (20)  -  (37).  That  is,  simply  subscript  the  following  variables  in  those  equations 
with  t:  x.  z.  F.  F  .  G.  H.  Q.  C.  R,  P.  \V. 

4.1.3  Derivation  of  Assimilation  Equations 

We  now  derive  the  equations  each  node  computes  to  assimilate  the  information  supplied 
by  otlmr  nodes.  References  to  equations  (29)  -  (37)  mean  their  local  versions.  From  the 
Assumption  above. 

m 

HtR-,(A-)H  =  ^[H^R.-'a-lH,]  (38) 

1  =  1 

ana  also 

m 

W((c)z(  k )  =  P(*  |  k)J^[HjR-'(k)z,{k)}.  (39) 

1=1 

From  (35)  we  can  say 

H^R-’H,  =  P;'(k+  1  |  k  +  l)  -  P~'(k  +  1  |  k).  (40) 

Hence  from  (35),  (38)  and  (40)  we  can  write 

•m 

P"‘(*  +  \  \k  +  \)  =  p-\k+  i  I  k)  +  ^[P;,(it+  i  I  k  +  1)-  p;\k+  l  |  *)]. 

7=1 
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ar.i  by  placing  this  assimilation  equation  at  each  node  (decentralizing)  we  arrive  at 


m 

p-’t h  -  i  i  k-+  D  =  p.-’u-  +  1 1  *■)  +  5Z[f»7’( a-  +  1 1  k  + 1)-  p -\k  +  1 1  k)}. 

3=1 

I  he  summation  over  j  does  not  include  the  term  when  j  =  i  since  this  has  already  been 
accounted  for  in  (33). 

Now  premultiplying  (40)  by  P ,( /c  +  1  |  k  -f  1 )  and  rearranging  g've.^ 

I  -  WtH,  -  P,(*  +1|H  l)Pt_1(^  +  1  |  k).  (41) 

Premultiplying  (44)  by  P~*(k  +  1  |  k  4-  1).  using  (41)  and  rearranging  gives 

HtrR* 1  z.( k  +  1)  =  P " 1  ( A-  +  1  |  k  +  l)x,(F  +  1  |  k  +  1)-  p-'(/t+  1  |  k)x,(k  +  1  |  k).  (42) 

Using  (34).  (41)  and  (42)  and  decentralizing  gives 


xd  k  +  1  1  k  -4-  1 )  —  Pt(  k  +  1  |  k  -F  1  )[P,  ( k  4-  1  j  k  )x;((r  4-  1  j  k ) 

rn 

+  ~  { Pj  1  ( k  +  1  |  k  4-  1  )Xj(  k  +  1  |  k  +  1 )  —  P;  1  (F  +  1  |  fc)x;(  k  +  1  |  k )}  ]. 

,i=i 

From  (32)  we  can  write 

G,C,R'’z,(F)  =  x,( k  +  1  |  k)  -  F,x,(F  |  k)  (43) 

and  noting  that  from  the  Assumption  we  can  write 

m 

GCR-'z^^C.C.R-’z,] 

i=i 

we  can  derive 

m 

x,(k  +  II  k)=  F ,(*)*,(*  |  k)+  £{*,-(*  +  1  |  k)-FJ(k)xJ(k+  1  j  *  +  1)). 

}=  i 

4.1,4  Assimilation  Equations 

To  reiterate:  each  node  takes  readings  from  its  sensor,  computes  its  local  version  of  equa¬ 
tions  33  -  34.  communicates  to  all  other  nodes  (see  below)  and  then  computes  the  equations 
given  below. 

Prediction: 

m 

x.r*  d  1  i  k  )  =  F ,( k )x,( k  I  k)+  £{*,(*  +  1  I  k)-fj(k)Xj(k+  1  I  k  +  1)}  (44) 

r=l 


2fi 


P.f A  4  1  |  A)  =  F,(A)P,(A  +  1  |  A  +  l)Fj(A-)  +  G,(A)Q,(A)Gf(A)  (-1') 


Update: 

x,(  A  +  1  |  A  4  i )  -  P , ( A  +  1  |  A  +  l)[P-,(*+  1  I  A)x,(A  +  1  |  A) 

4  jr  { P 7 1  f  A  +  ]  |  A  +  1  )x,(  A  +  1  |  A  +  1)  -  P“J(A  +  1  |  A)xJ(A  4  1  |  A)}  ]  (1C) 

1=1  - -  ^  ' 

state  error  info 


P)-’(A4  I  i  A  4  1)  =  P-’(A+  1  I  A)  +  ^{p;,(*+  1  I  A  +  1)  -  PJ 1  ( A  +  1  I  A)}  (47) 

j=i' - ■ - : - ' 

variance  error  injo 

u' in c  xiO  j  - 1 )  =  xo  and  P(0  |  - 1 )  =  P0  for  initialization  of  each  node. 

('1  he  summations  above  are  over  every  node  j  from  j  =  1  to  j  =  m  except  fnr  when 
j  =  M 

1  lie  terms  state  error  info  and  vario-ice  error  info  are  the  values  transmitted  by  earh 
node  to  each  other  node  during  the  communication  step  of  Fig.  12.  The  information  to 
be  communicated  ic  no*  complex  (one  vector  and  one  matrix  per  iteration),  and  the  data 
fusion  equation'  are  no  more  complex  than  the  local  estimate  update  equations. 


4.2  Implementation 

The  algorithms  have  been  implemented  on  a  SUN  3.0  computer  simulating  several  nodes 
running  simultaneously.  The  application  is  a  pursuer-evader  game  in  which  there  are  a 
number  of  independent  pursuers  attempting  to  surround  and  trap  a  moving  evadpr.  Th<= 
pursuers  and  evader  move  on  a  two  dimensional  surface:  the  evader  only  moves  horizontally 
and  with  a  constant  velocity.  Each  pursuer  is  an  independent  mobile  sensing  node  taking 
noisy  observations  of  the  single  moving  evader.  Each  pursuer  has  its  own  model  of  the 
system  (ie.  the  evader’s  motion  pattern)  and  they  all  communicate  between  themselves 
to  arrive  at  a  global  decision  of  the  location  of  the  evader  so  that  they  can  then  decide 
where  best  to  move  to  surround  it  (see  Fig.  13). 


4.2.1  Linear  Case 

In  this  case  each  pursuer  takes  full-state  z  =  (z,  y,  z)7  observations  of  the  evader,  corrupted 
by  noise  (the  noisiness  of  a  node's  readings  being  proportional  to  the  distance  between 
that  pursuer  and  the  evader)  and  each  has  a  correct  model  of  the  evader's  dynamics.  The 
evader  is  travelling  in  the  horizontal  direction  at  constant  velocity.  After  taking  readings 
the  pursuers  evaluate  local  versions  of  equations  33  -  34  and  then  calculate  the  data  they 
must  transmit  all  the  other  pursuers.  Each  communicates  this  data  to  each  other  pursuer 
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<a)  0>) 


Fieurc  13:  (a)  A  typical  starting  configuration  of  pursuers  and  the  evader,  (b)  The  pursuers  have 
surrounded  the  evader. 

and  collect  others'  variance  error  info  and  state  error  info  to  arrive  at  a  global  estimate 
for  the  (x,y)  position  of  the  evader  and  also  its  velocity  in  the  x  direction.  The  Kalman 
filter  is  initialized  by  setting  the  observed  velocity  of  the  evader  to  zero  for  the  first  five 
iterations  to  allow  the  filter  to  settle  before  the  noisy  observations  of  the  velocity  are  taken 
into  accoun t . 

4.2.2  Results 

Fig=.  14  and  15  are  graphs  of  the  global  estimates  from  two  pursuers  (only  two  are  shown 
for  clarity)  and  the  evader's  actual  parameters  plotted  against  number  of  iterations.  These 
results  show: 

1.  Each  node  arrives  at  the  same  (albeit  initially  inaccurate)  global  estimate  from 
the  very  first  iteration.  Therefore,  by  communication  the  evaders  have  reached 
a  common  group  consensus  of  the  position  of  the  evader. 

2.  The  global  estimates  for  the  (x,y)  positions  soon  converge  to  very  close  to 
the  actual  values  and  continue  to  track  it  well.  The  filter  converges  faster 
than  a  conventional  Kalman  Filter  since  it  is  able  to  process  m  lots  of  sensors' 
information  simultaneously. 

3.  The  global  estimate  for  the  velocity  of  the  evader  x  converges  more  slowly  to 
the  actual  value  but  this  is  to  be  expected  since  x  is  small  compared  to  the 
observation  noise  and  also  differentiation  always  amplifies  noise. 
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4.2.3  Nonlinear  Case 


In  tlii^  racr  each  pursuer  has  an  (r.P.r.P)  state  representation  of  the  position  of  tlm  evader, 
whieh  is  moving  jtl  the  horizontal  direction  with  a  constant  velocity.  However,  in  order 
to  simulate  a  realistic  case  where  each  pursuer  is  a  photodiode  based  device,  angle  only 
measurement*  an  taken.  No  range  data  is  available  from  the  sensors  so  communication 
between  the  pursuer  is  essential  for  pursuers  to  arrive  at  a  full  state  vector.  Each  pursuer 
takes  its  sensor  measurement  and  constructs  its  own  view  of  the  state  of  the  object. 
Each  pursuer  knows  where  it  is  and  where  all  the  other  pursuers  were  from  tlm  last 
communication  step  so  each  pursuer  is  able  to  transmit  appropriately  transformed  variance 
error  and  state  error  information  to  the  others.  (Also  each  pursuer  transmits  its  current 
position  to  all  the  others).  The  assimilation  equations  are  computed  and  each  pursue- 
ends  up  with  a  full  state  representation  of  the  evader.  This  ‘filling  in'  of  the  empty  states 
occurs  because  if  one  computes  the  linear  weighted  sum  of  a  node's  state  estimate  x,  and 
another  node's  transformed  estimate  'Xj  the  new  state  vector  x,  obtained  is  full  rank  (ie. 
the  intersection  of  the  angle  estimates  of  t he  two  nodes  has  been  solved  to  give  range  data) 
[f)j.  If 

x,  =  (,Erx,  +  JtET‘Xj)(,i:T  + 

where  x,  and  x,  are  (r.0)  state  vectors  with  initially  no  values  for  r  and  and 
are  the  associated  information  matrices  with  zeroes  in  the  range  information  positions 
one  obtains  the  correct  estimate  for  r  in  x,.  The  filter  required  five  steps  to  arrive  at 
reasonably  accurate  estimates  for  the  r  and  f  terms  for  each  pursuer.  Then  it  was  able  to 
follow  the  evader,  updating  its  estimates  for  f  and  6  on  each  iteration. 


4.2.4  Results 

Results  are  shown  in  Fig.  15.  which  shows  the  (x,y)  position  of  the  evader  and  the  (r.j/) 
estimates  of  two  pursuers.  Only  two  pursuers'  estimates  are  shown  for  clarity.  The  [r.P) 
state  representations  of  each  pursuer  have  been  converted  to  cartesian  form  for  these  plots, 
simply  so  that  the  estimates  of  two  pursuers  may  be  directly  compared.  The  results  show 
that: 

3.  Tim  pursuers  are  able  to  track  the  evader  despite  each  pursuer  taking  only  a 
partial  estimate  of  the  state.  By  communication  between  themselves  they  are 
able  to  arrive  at  a  full  state  vector  estimate  for  the  position  of  the  evader. 

2.  The  pursuers  are  in  agreement  over  the  position  of  the  evader.  The  non- 
linearities  in  the  system  (ie,  using  an  (r,0)  representation  for  a  linear  cartesian 
motion)  cause  the  slight  error  between  the  two  pursuers'  estimates. 

4.3  The  Future 

The  algorithm  has  successfully  been  implemented  in  OCCAM  on  an  array  of  sensors,  each 
with  a  photodiode  based  angle  detector  and  its  own  transputer.  The  algorithm  is  being 


30 


tested  on  real  data,  in  experiments  with  sensors  returning  measurements  a«yiu hmnr.us!-,-. 
Also  the  algorithm  may  l>o  extended  to  allow  tracking  of  several  objects  simultaneously. 

Tim  motivation  behind  this  work  was  to  attempt  to  discover  and  set  down  mathematically 
what  pieces  of  information  inuct  be  communicated  between  a  group  of  sensors  in  order  for 
them  to  reach  a  conclusion.  This  problem  must  now  be  investigated  further  and  beyond 
the  limits  of  just  a  simple  linear  tracking  filter  algorithm.  It  appears  that  the  multi¬ 
sensor  network  we  have  analysed  here  may  be  thought  of  as  a  complex  Markov  chain  with 
information  propagating  through  each  node.  This  line  of  research  borders  onto  the  wider 
issue  of  sensor  models  and  may  throw  some  light  on  how  best  to  mode]  complex  sensors 
such  as  CCD  cameras  in  order  that  the  value  oh  the  information  returned  by  a  sensor  in 
anv  arbitrary  situation  may  be  assured  by  other  sensors.  Being  able  to  model  sensors  in 
this  way  would  lead  to  the  possibility  of  building  intelligent  sensing  nodes  that  could  he 
connected  together  as  a  fully  decentralized  network. 


5  DISCUSSION 

5.1  Optimal  Estimation  and  Active  Intelligence 

Optimal  estimation  techniques  have  at  least  three  distinct  roles  to  play  in  real-time  sen¬ 
sorimotor  systems. 

1.  They  can  be  used  a^  the  basic  paradigm  for  estimating  the  state  of  systems  internal 
to  the  observer.  Estimating  external  stales  is  akin  to  perception. 

2.  They  can  he  used  to  estimate  tlm  state  of  systems  internal  to  the  observer.  Estimat¬ 
ing  internal  state  involves  aspects  of  proprioception  (using  information  from  internal 
sensors),  but  can  also  involve  sensing  the  outside  world,  especially  to  determine 
dynamic  observer  parameters  such  as  location  and  velocity. 

3.  They  can  be  used  as  low-level  utilities  in  service  of  several  aspects  of  perception  or 
action. 

5.1.1  Control  Styles  in  Perception 

Top-doun  (expectation-driven,  hypothesis-verification)  methods  cope  with  the  inherent 
underdetermined  and  computationally  intensive  nature  of  perception  by  using  apriori 
knowledge  to  constrain  the  space  of  interpretations  for  perceptual  data.  In  a  naviga¬ 
tion  context,  these  methods  correspond  to  map-guided  route-finding,  perhaps  landmark 
recognition  and  similar  tasks  in  which  an  internal  model  exists  and  the  input  is  expected. 

One  important  role  of  perception  is  to  cope  with  the  unexpected.  This  seeming  truism 
is  often  ignored,  and  has  deep  implications  for  computational  perceptual  models.  In  par¬ 
ticular  it  implies  that  tops-down  control  strategies  are  by  themselves  inadequate.  In  a 
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navigational  context,  obstacle  avoidance  illustrates  this  role.  The  complementary  control 
strategv  is  bottom-up.  or  data-driven:  Here  the  style  is  often  a  fixed  order  of  processing  of 
input  data  (sav  hv  successive  levels  of  feature  detection  and  extraction)  leading  to  increas- 
inglv  abstract  levels  of  description  of  the  input.  As  technology  improves  it  is  becoming 
possible  to  achieve  the  massive  data-processing  effort  in  real  time,  and  the  practical  con¬ 
siderations  that  have  partiallv  motivated  the  tops-down  approach  are  vanishing  (sec,  c.g. 
[6])- 

In  one  sense,  the  Kalman  filter  is  an  example  of  expectation-driven  perception.  By  defi¬ 
nition  it  incorporates  explicit  models  of  dynamics  and  noise.  The  strength  of  the  Kalman 
filter  for  estimation  is  that  it  has  these  models  at  its  disposal,  but  requiring  them  limits 
the  sorts  of  perceptual  jobs  that  the  Kalman  filter  can  reasonably  be  expected  to  perform. 
The  severe  tops-down  requirements  can  be  mitigated  to  some  extent,  and  at  some  cost, 
bv  such  measures  as  running  several  different  filters  embodying  different  assumptions  in 
parallel,  switching  between  filters  when  lack  of  fit  motivates  such  a  switch,  allowing  the 
filter  to  estimate  control  inputs  to  the  plant,  etc.,  as  we  have  seen  in  earlier  sections. 

])ecpito  such  seemingly  sophisticated  adaptive  capabilities,  the  extensive  literature  on 
Kalman  filtering  applications  (e.g.  (3.25, 1.2, 12. 14. 18,22,23. 10J  reveals  that  the  perceptual 
tasks  most  often  attempted  are  those  in  which  the  plant  (often  target )  follows  well-known 
and  rather  simple  (e.g.  ballistic)  dynamic  laws,  and  in  which  the  target  is  modeled  as  a 
point  in  space.  The  typical  perceptual  task  is  tracking  the  (perhaps  maneuvering)  target 
(perhaps  in  clutter).  Thus  the  perceptual  task  ( data  association .  or  segmentation),  consists 
of  the  twofold  problem  oflinking  measurements  together  into  tracks  and  ignoring  spurious 
data.  Tim  basic  Kalman  filter  mechanism  provides  help  in  the  way  of  quantified  measures 
of  uncertainty,  surprise,  information,  expectation,  etc.  but  provides  nothing  directly  to 
cope  with  the  familiar  problems  of  controlling  search  in  interpretation  space.  The  track- 
splitting,  NNSF.  PDAF.  etc.  approaches  all  have  analogs  in  the  edge-linking  problem  in 
computer  vision,  for  example. 

Dissatisfaction  with  the  paradigm  of  expectation-driven  low-level  perception,  combined 
with  skepticism  about  the  efficacy  of  local  bottom-up  segmentation,  has  motivated  other 
algorithm'  for  target  tracking.  Their  goal  is  often  to  accomplish  perceptual  grouping  using 
global,  expectation-driven  metrics  of  grouping  quality,  as  well  as  to  cope  with  input  in  a 
more  data-driven  way  ( 1 9] .  However,  much  remains  to  be  done  here. 


5.1.2  Perception:  Estimating  External  State 

It  seems  that  if  optimal  estimation  techniques  are  to  be  a  paradigm  for  perception,  at  least 
the  following  conditions  must  apply. 

1.  The  dynamics  of  the  objects  must  obey  known,  predictable  laws. 

2.  The  noise  mean  and  covariance  properties  of  the  domain  dynamics  and  of  the  mea¬ 
surement  system  must  be  known  apriori. 
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3.  Tim  data  may  arise  from  several  information  sources  —  the  Kalman  filtering  tech 
niqup  provides  a  principled  way  to  combine  (fuse)  them. 

4.  The  raw  perceptual  input  must  he  processed  to  yield  a  measurement  vector  con¬ 
taining  information  about  the  state  of  the  observed  objects.  If  objects  arn  more 
complicated  than  single  points,  t his  step  may  call  for  solving  "the  vision  problem" 
in  order  to  do  tracking.  An  extreme  example  is  reliable  tracking  of  a  face  in  a  crowd, 
using  data  from  a  face-recognizer.  The  point  is  that  even  such  basic  vision  tasks 
as  region-finding  are  not  well  understood,  and  should  not  be  lightly  suggested  as 
"preprocessing"  for  a  tracker. 

5.  Measurement  data  must  be  available  over  a  significant  interval,  probably  tens  of 
time-steps  for  reasonably  complex  domain  dynamics. 

6.  Dealing  with  complex  perceptual  events  in  real  time  will  call  for  substantial  compu¬ 
tational  resources. 

5.1.3  Proprioception:  Estimating  Internal  State 

The  oilier  main  use  for  Kalman  filters  is  for  internal  state  estimation,  in  aid  of  complex, 
often  adaptive,  control  (e.g..  [10]).  Such  proprioception  is  not  divorced  from  perception:  A 
vehicle  can  determine  its  position  from  fixed  beacons  (say  landmarks  or  stars)  by  tracking 
them  and  interpreting  the  data  with  a  Kalman  filter.  In  practice,  the  contrast  between  the 
sophistication  of  the  dynamic  models  for  plants  that  are  to  be  estimated  and  controlled 
and  the  models  of  external  targets  is  dramatic  (for  a  plant  to  be  controlled,  sometime' 
80  state  variables,  for  a  target  to  be  tracked,  perhaps  6ix).  Presumably  the  observer's 
state  description  equations  are  relatively  stable,  and  devoting  computational  and  analytic 
resources  to  precise  observer  description  is  a  good  long-term  investment  that  will  pay  off 
in  better  information  about  and  control  over  its  state.  For  another  thing,  the  observables 
themselves  am  under  more  control.  A  roving  vehicle  can  observe  bar-coded  reflectors  as 
location  beacons,  or  can  track  static  features  such  as  walls  or  furniture,  whose  apparent 
motion  arises  more  or  less  predictably  from  observer  motion. 

Thus  tlm  proprioception  problem  is  inherently  more  "expectation  driven"  than  the  per¬ 
ception  problem,  and  Kalman  filtering  techniques  may  well  be  an  appropriate  paradigm 
since  the  following  conditions  apply. 

1.  The  dynamics  of  the  observer  obeys  known,  predictable  laws.  They  may  be  complex 
to  model  but  there  is  only  a  single,  known  system  to  characterise,  and  it  makes  sense 
that  the  observer  is  something  the  observer  itself  knows  best. 

2.  Proprioception  may  be  provided  by  on-board  sensors  such  as  tachometers,  odome¬ 
ters,  shaft  encoders,  etc. 

3.  The  noise  mean  and  covariance  properties  of  the  observer  dynamics  and  its  mea¬ 
surement  system  can  be  experimentally  determined  "off  line",  as  can  the  properties 
of  the  expected  visual  stimuli. 
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■1.  Tim  raw  perceptual  input  must  he  processed  to  yield  a  measurement  vector  con 
taining  information  about  the  state  of  the  observer,  but  the  observer  can  choose  to 
interpret  a  limited  set  of  stimuli  by  customized  methods  if  internal  state  estimation 
is  the  only  goal. 

5.  Presumable  measurement  data  is  available  for  a  significant  interval,  on  the  order  of 
the  "lifetime"  of  the  observer,  rather  than  of  the  lifetime  of  the  unexpected  visual 
phenomena  that  occur  in  perception. 

6.  Computational  requirements  may  not  be  as  severe  since  the  update  rate  needed  for 
proprioception  may  be  lower  than  that  for  perception. 

5.2  The  Future 

We  plan  to  use  both  aspects  of  estimation  in  work  at  Rochester  to  integrate  real-time 
vision  and  motion  with  high-level  planning  in  a  hierarchical  parallel  system.  An  early  stf'p 
lias  bf'mi  the  inclusion  of  estimation  techniques  in  the  robot's  gaze  control  system  [4,r>j. 
This  woik  is  in  the  simulation  stage,  but  a  simulator  is  needed  in  any  event  to  implement 
the  Smith  predictor  used  to  cope  with  software  delays  in  the  system.  The  simulator  uses  a 
suboptimal.  time-invariant  filler  (the  a  —  d  filter  [3]  )  on  the  two-dimensional  imasm  data 
and  the  three-dimensional  target  location  data  presumed  available  from  binocular  stereo, 
kinetic  depth  calculations,  or  other  means. 


6  APPENDIX 

6.1  The  Kalman  Filter  Utilities 

Tim  EKF  utilities  are  supported  by  a  standard  suite  of  matrix  routines.  The  filter,  plant 
and  observer  are  implemented  as  structures.  There  is  very  little  use  of  global  storage: 
instead  there  is  storage  attached  to  structures,  and  static  working  storage  within  routines. 
Filters  may  currently  be  connected  in  linked  lists,  and  (should  the  need  arise)  the  linking 
mechanism  is  easy  to  extend  to  support  trees  and  forests  of  filters.  The  code  is  in  several 
files,  some  of  which  should  not  need  to  change  with  the  application,  some  of  which  must. 
They  are  available  in  cmb/src/ekf.  The  PDAF  application  (the  most  recent)  is  in  .../pdaf. 
also  needed  are  various  header  files  in  cmb/include  and  the  statistical  and  matrix  utilities 
in  cmb/lib. 

0.1.1  Sample  Main  Loop 

The  main  control  loop  for  the  example  of  the  variable  dimension  filter  looks  approximately 
like  the  following  code  fragment. 

fquies  =  creat_f ilt(Quies_X_Dim, ,Quies_Z_Dim,Qname) ; 
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fmaneu  =  cr. f  ilter  (Maneu. X.Dim ,  Maneu.Z.Dim ,  Mname )  ; 

p  =  create. plant (Maneu.X. Dim, .Pname) ; 

o  =  create.obs (Maneu. X.Dim ,  Maneu. Z.Dim.Oname) ; 

init.filter(fquies,p,o) ; 

current.!  =  fquies; 

current.time  =  0.0; 

for(i=0;i<steps;  i++) 

{ 

current.time  +=  timestep; 
delta. plant(p) ; 
measure (p  ,  o) ; 
ekf(current_f,p,o) ; 

report (current.! , p) ;  /‘simulation  output*/ 
if  (current.f .  norm._innov  >  thresh) 

{ 

init.f 2(fmaneu , current.f ,p,o) ; 
current.f  =  fmaneu; 

> 

> 

Thp  basic  loop  ran  he  embedded  in  an  interactive,  window-based  environment  or  ran 
simply  accept  keyboard  input  and  output  data  to  files. 

6.1.2  Data  Structures 


extern  struct  Filter; 

typedef  struct  Filter 
{ 

int  XDIM ; 
int  ZDIM ; 
int  name ; 
matrix.t  H; 
matrix.t  F; 

not  be  used  in  extended 
f  and  h  functions  */ 
matrix.t  E; 
matrix.t  W; 
matrix.t  S; 
matrix.t  Q; 
matrix.t  P.est; 


/‘plant  state  dimension*/ 

/‘measurement  dimension*/ 

/‘filter  ID*/ 

/‘measurement  matrix*/ 

/‘plant  dynamics  matrix  --  F  and  H  may 
filter . . . need 

/‘measurement  noise  covariance*/ 
/‘Kalman  gain  matrix*/ 

/‘Innovation  covariance*/ 

/‘State  noise  covariance*/ 

/‘predicted  state  error  covariance*/ 
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matrix.t  xhat.est;  /‘predicted  state*/ 

matrix.t  rho ;  /‘fading  memory  likelihood  function*/ 
matrix. t  norm.staterr ;  /‘norm,  state  err.  for  sims*/ 
matrix. t  norm.innov;  /‘normalized  innovation*/ 

double  alpha;  /‘ratio  for  fading  memory*/ 

matrix. t  (‘compute. H) () ;  /*  funcs  for  ekf  matrices*/ 
matrix. t  (‘compute.F) () ; 
matrix. t  (‘compute. R) () ; 

struct  Filter  ‘next;  /‘filter  linking  information*/ 
struct  Filter  ‘last; 

>  ‘Filter.t; 


typedef  struct  plant 

{ 


/*  redesigned  for  each  plant*/ 


int  name; 
int  XDIM ; 
matrix. t  F ; 
matrix.t  x; 
matrix. t  V; 
matrix.t  pure.x; 
double  sqrtq; 

}  ‘plant. t; 


/‘plant  ID*/ 

/‘plant  state  dimension*/ 
/‘plant  dynamics*/ 

/‘plant  state*/ 

/♦plant  noise  vector*/ 
/‘noiseless  state*/ 

/‘noise  standard  deviation*/ 


typedef  struct  observer  /*  redesigned  for  each  observer*/ 

int  name; 
int  OBSXDIM ; 
int  ZDIM; 

matrix.t  obs.x;  /‘observed  x*/ 
matrix.t  obs.x.pure;  /‘noiseless  observed  x*/ 
double  sqrtrx; 
double  sqrtry; 

double  sqrtrs;  /‘noise  standard  devs*/ 
matrix.t  v; 
matrix.t  z; 

matrix.t  pure.z;  /‘noiseless  observation*/ 

}  ‘observer.t; 

6.2  Computational  Effort 


Here  we  assess  the  computational  requirements  of  the  current  implementation  of  the 
Kalman  filter.  Let  A'  be  the  dimensionality  of  the  state  vector  and  Z  that  of  the  measure¬ 
ment  vector. 
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1 1;  i ;  i  a !  i  7 :  ti  c  a  filter  allocate  memory  for  3!  mat  rice?,  varying  in  size  from  A  x  A  to  1  x  1 . 
At  each  iteration  of  tlm  nonlinear  (extended)  Kalman  filter,  procedure?  must  he  run  to 
evaluate  hi.)  (the  measurement  equation),  to  predict  the  measurement,  and  to  compute 
ft  (the  measurement  covariance),  both  of  which  can  he  function?  of  time.  In  the  linear 
filter  tlm  II  and  R  matrices  are  fixed  during  initialization. 

During  one  iteration  of  the  Kalman  filter,  as  currently  implemented,  there  are  some  2  s 
separate  matrix  operation?  (transpose,  add.  multiply,  inverse).  The  primitive  operation 
count  for  the  matrix  operations  in  the  (linear)  Kalman  filter  is  as  follows. 

Multiplies  =  2  A' 3  +  2Z3  +  3X7Z  +  2XZ7  +  A'2  +  Z2  +  2AZ  +  Z 
Adds  =  2 A  3  +  2Z3  +  3  A '2Z  +  3  A  Z2  +  3 A' 2  +  2Z2  +  2.AZ  +  2Z  +  A' 

Copies  =  A  +  3A’  Z  \  Z 

If  the  ratio  of  execution  times  for  multiplication  to  addition  to  copv  is  5:1:0,  A  =  1,  and 
7  —  2.  each  filter  iteration  costs  the  equivalent  of  1918  additions.  In  that  time  rum  ran 
perform  thr  equivalent  of  a  "5  x  5"  Fast  Fourier  Transform  (disregarding  powers  of-two 
sizing'.  If  A  =  10.  Z  =  5,  an  equivalent  of  27.370  additions  is  required,  which  is  the  effort 
for  a  "  1  3  x  13"  I  F  I'.  Nineteen  steps  of  the  larger  Kalman  filter  or  273  steps  of  the  smaller 
can  run  in  the  time  needed  for  a  256  x  256  FFT. 

The  implementation  above  is  the  most  straightforward  (i.e.  naive)  one  its  author  (Drown ) 
could  imagine.  Numerically  more  robust  methods,  which  can  provide  faster  performance 
a?  well,  include  a  whole  range  of  "square  root"  methods.  Gelb  ([12].  p.  lOGff.)  discusses 
them  and  also  presents  computer  loading  and  timing  analysis.  Ron  Daniel  of  the  RRG 
has  implemented  ingenious  methods  for  steady-state  Kalman  filtering  on  modern  processor 
chips  (MGxn.qq,  that  car,  run  at  800  Hz. 
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